% sim_QPSK_rx
% QPSK发射机、信道、接收机全流程matlab定点化仿真 2022.2.25
% 发射机：
%       导频头、帧同步头、信息形成tx信号(2.5Msps)，4倍过采样(10Mbps)，升余弦成型滤波，farrow内插至60MHz
% 信道：
%       模拟非平坦信道，awgn加噪
% 接收机：
%       6倍降采样(60MHz->10MHz)，升余弦匹配成型滤波，PLL载波同步，Gardner位同步，logAGC，均衡，帧同步

clear;
clc;
close all;
disp('仿真开始.');

%% init
[setting,agc,gardner,pll,detection,frameSync,eq] = init();

%% tx 
rcos_fir = floor(rcosdesign(setting.Rcos_Beta,setting.Rcos_span,...
            setting.OSR,'sqrt')*2^15);
tx_pilot_single = m_sequence([1, 0, 0, 0, 0, 1]);
tx_pilot_shape = floor(upfirdn(tx_pilot_single,rcos_fir,setting.OSR)/16);
tx_pilot = repmat(tx_pilot_single, setting.Pilot_Time, 1);
tx_frameSync = m_sequence([0, 1, 0, 0, 1]);
tx_frameSync_90 = tx_frameSync*exp(1i*pi/2);
tx_frameSync_180 = tx_frameSync*exp(1i*pi);
tx_frameSync_270 = tx_frameSync*exp(1i*3*pi/2);
tx_msg = [];
for tx_index = 1:100
    msg = sqrt(2)*exp(1i*(2*pi*randi([0,setting.M-1],setting.Msg_Len,1)/setting.M+pi/4));
    tx_msg = [tx_msg;tx_pilot;tx_frameSync;msg];
end

tx_shape = floor(upfirdn(tx_msg,rcos_fir,setting.OSR)/16);
tx_farrow = farrow_int(10e6,setting.Fs,tx_shape);
t = ((0:length(tx_farrow)-1)/setting.Fs)';
tx_carrier = exp(1i*2*pi*setting.Fd*t);
tx = tx_farrow .*tx_carrier;

%% AWGN channel
chann_in = [zeros(randi([20000,50000]),1);tx];
chann_out = awgn(chann_in,setting.SNR,'measured');
chann_out = floor(chann_out/max(abs(chann_out))*2^11);
%% rx
rx_downsample = downsample(chann_out,6);
rx_shape = floor(conv(rcos_fir,rx_downsample)/2^16);
% debug
before_eq = [];
after_eq = [];
eq_e_temp = [];
agc_watch = [];
pll_watch = [];
gardner_res = [];
c_0_watch = [];
c_90_watch = [];
c_180_watch = [];
c_270_watch = [];
% load('matlab.mat');
for rx_index = 1:length(rx_shape)
    %% AGC，对接收到的信号进行AGC自动增益控制使AGC之后的功率能够到达2^11级别
    agc.window = [agc.window(2:end),...
        (real(rx_shape(rx_index))^2+imag(rx_shape(rx_index))^2)/2^11];
    if(rx_index>length(agc.window))  
        z = log(mean(agc.window)/2^11)*2^11;
        agc.output = rx_shape(rx_index)*exp(agc.g/2^11);
        agc.g = agc.g + agc.K*(agc.A-(z+2*agc.g));
        
        agc.out_window = [agc.out_window(2:end),...
                (real(agc.output)^2+imag(agc.output)^2)/2^11];
        if(agc.flag_steady==0)
            if(mean(agc.out_window) <= 2200 && mean(agc.out_window)>=1800)
                agc.cnt_s = agc.cnt_s +1;
                if(agc.cnt_s == agc.cnt_s_target)
                    agc.flag_steady = 1;
                    disp('AGC稳定');
                end
            end
        end
    end
    %% 通过对导频头进行相关运算检测当前是否有信号
    if(agc.flag_steady==1 && detection.flag_detected==0)
        detection.input_window = [detection.input_window(2:end),...
                                    agc.output];
        c = real(conj(detection.input_window) * tx_pilot_shape);
        detection.c_window = [detection.c_window(2:end);c];
        detection.window_cnt = detection.window_cnt + 1;
        if(detection.window_cnt > detection.Window_Len)
            parp = max(detection.c_window) / mean(abs(detection.c_window));
            if(parp > detection.PARP_Threshold)
                detection.cnt = detection.cnt + 1;
            end
        end
        if(detection.cnt == detection.A)
            detection.flag_detected = 1;
            disp('检测到信号');
        end
    end
    %% costas环载波同步
    if(detection.flag_detected==1)
        pll.phase = mod(pll.phase + pll.ftw,2^32);
        pll.output = agc.output * exp(-1i*2*pi*pll.phase/2^32);
        pll.phase_error = imag(pll.output)*sign(real(pll.output))...
                            -real(pll.output)*sign(imag(pll.output));
        pll.loopfilter = pll.loopfilter + pll.phase_error;
        % 3 8-更加稳定
        % 2 10-更加激进
        pll.ftw = floor(pll.loopfilter * 2^-2)...
                    + floor(pll.phase_error * 2^10);
%         pll.watch.phase = [pll.watch.phase;pll.phase];
%         pll.watch.filter = [pll.watch.filter;pll.loopfilter];
        % 检查pll是否锁定
        if(pll.flag_locked == 0)
            pll.filter_window = [pll.filter_window(2:end),pll.loopfilter];
            if(max(abs(pll.filter_window-mean(pll.filter_window)))<pll.Threshold)
                pll.lock_cnt = pll.lock_cnt + 1;
                if(pll.lock_cnt == pll.Lock_target)
                    pll.flag_locked = 1;
                    disp('pll锁定.');
                end
            end
        end
    end
    %% gardner环位同步
    if(pll.flag_locked==1)
        gardner.i_valid = ~gardner.i_valid;
        gardner.o_valid = 0;
        if(gardner.i_valid==1)
            gardner.input = [pll.output;gardner.input(1:end-1)];
            gardner.cnt = gardner.cnt_next;
            gardner.mu = gardner.mu_next;
            v2 = 1/2*[1,-1,-1,1]*gardner.input;
            v1 = 1/2*[-1,3,-1,-1]*gardner.input;
            v0 = gardner.input(3);
            gardner.output = (gardner.mu*v2+v1)*gardner.mu+v0; 
            if(gardner.underflow==1)
                e = real(conj(gardner.TEDBuffer(1)) * (gardner.TEDBuffer(2)-gardner.output)/2^11);
            else
                e = 0;
            end
            if(gardner.flag_locked==1 && gardner.underflow==1)
                gardner.o_valid = 1;  
            end
            gardner.vp = gardner.k1 * e;
            gardner.vi = gardner.vi + gardner.k2*e;
            gardner.v = gardner.vp + gardner.vi;
            gardner.w = 2^14 + gardner.v;
%             gardner.watch.v = [gardner.watch.v;gardner.mu];

            gardner.cnt_next =  gardner.cnt -  gardner.w;
            if( gardner.cnt_next < 0)
                gardner.cnt_next = 32768 + gardner.cnt_next;
                gardner.underflow = 1;
                gardner.mu_next = gardner.cnt/gardner.w;
            else
                gardner.underflow = 0;
                gardner.mu_next = gardner.mu;
            end
            gardner.TEDBuffer = [gardner.output; gardner.TEDBuffer(1)];
            
            gardner.mu_cnt = gardner.mu_cnt + 1;
            gardner.mu_window = [gardner.mu_window(2:end);gardner.mu];
            % 检查gardner是否稳定
            if(gardner.mu_cnt >= length(gardner.mu_window))
                if(max(abs(gardner.mu_window-mean(gardner.mu_window))) < gardner.Threshold)
                    gardner.lock_cnt = gardner.lock_cnt + 1;
                    if(gardner.lock_cnt == 1000)
                        gardner.flag_locked = 1;
                        disp(['gardner锁定at',num2str(gardner.mu_cnt)]);
                    end
                end
            end
        end
    end
    
    %% LMS均衡
    if(gardner.flag_locked==1 && gardner.o_valid==1)
        gardner_res = [gardner_res;gardner.output];
        eq.input = [gardner.output/2^11;eq.input(1:end-1)];
        eq.output = eq.weight' * eq.input;
%         after_eq = [after_eq;eq.output];
        frameSync.window = [frameSync.window(2:end), eq.output];
%         c_0_watch = [c_0_watch;real(conj(frameSync.window)*tx_frameSync)];
%         c_90_watch = [c_90_watch;real(conj(frameSync.window)*tx_frameSync_90)];
%         c_180_watch = [c_180_watch;real(conj(frameSync.window)*tx_frameSync_180)];
%         c_270_watch = [c_270_watch;real(conj(frameSync.window)*tx_frameSync_270)];
        if(real(conj(frameSync.window)*tx_frameSync) > frameSync.Threshold)
%             disp('pll 0° offset.');
            eq.d = tx_frameSync;
            frameSync.detected = 1;
        elseif(real(conj(frameSync.window)*tx_frameSync_90) > frameSync.Threshold)
%             disp('pll 90° offset.');
            eq.d = tx_frameSync_90;
            frameSync.detected = 1;
        elseif(real(conj(frameSync.window)*tx_frameSync_180) > frameSync.Threshold)
%             disp('pll 180° offset.');
            eq.d = tx_frameSync_180;
            frameSync.detected = 1;
        elseif(real(conj(frameSync.window)*tx_frameSync_270) > frameSync.Threshold)
%             disp('pll 270° offset');
            eq.d = tx_frameSync_270;
            frameSync.detected = 1;
        end
        if(frameSync.detected == 1)
            temp_in = [zeros(eq.Ref_Len,1);conj(frameSync.window');zeros(eq.Ref_Len-1,1)];
            for index = 1:setting.FrameSync_Len
                temp_out = eq.weight' * temp_in(index+eq.Tap_Len-1:-1:index);
                eq.e = eq.d(index) - temp_out;
                eq.weight = eq.weight + eq.mu * conj(eq.e) * temp_in(index+eq.Tap_Len-1:-1:index);
                eq_e_temp = [eq_e_temp;abs(eq.e)];
            end
            frameSync.detected = 0;
        end
    end
end
% figure;plot(pll.watch.phase);title('pll环路相位变化');
% figure;plot(pll.watch.filter);title('pll环路积分器变化');
figure;plot(eq_e_temp);title('均衡误差迭代图');
% 
% a = [];
% for i = 512:length(gardner.watch.v)
%     a = [a;max(abs(gardner.watch.v(i:-1:i-511)-mean(gardner.watch.v(i:-1:i-511))))];
% end
figure;plot(gardner_res(1:end-100),'b*');
hold on;plot(after_eq(15000:end-2000)*2^11,'rs');
title('均衡前后星座图对比');legend('均衡前','均衡后');
% figure;
% subplot(221);plot(c_0_watch); 
% subplot(222);plot(c_90_watch);
% subplot(223);plot(c_180_watch);
% subplot(224);plot(c_270_watch);